#include "WProgram.h"
// Experiment with different ways to measure tilt angle
#define BAUD 9600

// Define pin assignments
#define LEDPIN 13
#define XACCELPIN 0 // I miswired my board, change order based on your setup
#define YACCELPIN 1
#define ZACCELPIN 2
#define XRATEPIN 4
#define YRATEPIN 3
#define ZRATEPIN 5

// Accelerometer setup
// these A/D values depend on how well the sensors are mounted
// change these values to your unique configuration
#define XMIN 405
#define XMAX 607
#define YMIN 409
#define YMAX 618
#define ZMIN 403
#define ZMAX 611

// Accelerometer setup
void setup();
void loop();
float findZero(int channel);
void xmitData();
int smooth(int data, float filterVal, float smoothedVal);
float xAccMid = ((float)(XMAX-XMIN) / 2) + (float)XMIN;
float yAccMid = ((float)(YMAX-YMIN) / 2) + (float)YMIN;
float zAccMid = ((float)(ZMAX-ZMIN) / 2) + (float)ZMIN;
float xAccScaleFactor = 1 / (float)(XMAX - XMIN) * 2;
float yAccScaleFactor = 1 / (float)(YMAX - YMIN) * 2;
float zAccScaleFactor = 1 / (float)(ZMAX - ZMIN) * 2;
int xAccZero, yAccZero, zAccZero;
float xG, yG, zG;

// Alternate accelerometer setup
// gx = mx(X) + bx, where X is analogRead() value for x accelerometer
float gx, gy, gz;
float mx = 2 / float(XMAX - XMIN);
float bx = 1 - float(mx * XMAX);
float my = 2 / float(YMAX - YMIN);
float by = 1 - float(my * YMAX);
float mz = 2 / float(ZMAX - ZMIN);
float bz = 1 - float(mz * ZMAX);
// rho = pitch angle of x-axis relative to the ground
// phi = roll angle of y-axis relative to the ground
// theta = angle of z-axis relative to gravity
float rho, phi, theta;
float rho_alt, phi_alt, theta_alt;
float PiOver2 = PI/2;

// Gyro setup
// if AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
float GyroMid = 465;
// gyro range is from +/-500 deg/sec
float GyroScaleFactor = 1001 / (float)ZMAX;
int xGyroZero, yGyroZero, zGyroZero;
float xRate, yRate, zRate;

unsigned long previousTime = 0;
unsigned long deltaTime1 = 0;
unsigned long deltaTime2 = 0;
int x, y, z;

float filterVal = 0.001;       // this determines smoothness  - .0001 is max  1 is off (no smoothing)
float smoothedVal;     // this holds the last loop value just use a unique variable for every different sensor that needs smoothing


void setup() {
  digitalWrite(LEDPIN, HIGH);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  pinMode(LEDPIN, OUTPUT);
  Serial.begin(BAUD);
  xAccZero = xAccMid - findZero(XACCELPIN);
  yAccZero = yAccMid - findZero(YACCELPIN);
  zAccZero = findZero(ZACCELPIN) - ZMAX;
  xGyroZero = GyroMid - findZero(XRATEPIN);
  yGyroZero = GyroMid - findZero(YRATEPIN);
  zGyroZero = GyroMid - findZero(ZRATEPIN);
}

void loop() {

  x = smooth(analogRead(XACCELPIN), filterVal, smoothedVal);
  y = analogRead(YACCELPIN);
  z = analogRead(ZACCELPIN);
  
  previousTime = millis();

  // G = ((ACC-(((MAX-MIN)/2)+MIN)+ZERO) * (1/(MAX-MIN)*2)
  xG = (x - xAccMid + xAccZero) * xAccScaleFactor;
  yG = (y - yAccMid + yAccZero) * yAccScaleFactor;
  zG = (z - zAccMid + zAccZero) * zAccScaleFactor;
  
  // roll is arctan(az/ay)-PI/2 or arctan2(az, ay)-PI/2
  //phi = atan2(zG, yG)-PiOver2;
  phi = atan2(yG, zG);
  rho = atan2(xG, zG);
  if (zG < 0) {
    if (phi > PI/2) rho -= PI;// + (PI/2);
    else if (phi < -PI/2) rho += PI;
    //if (rho < PI/2) phi += PI;
    //else if (rho > PI/2) phi -= PI;
  }// + (PI/2);
  // pitch is arctan(az/ax)-PI/2 or arctan2(az, ax)-PI/2
  //if (zG > 0) rho = atan2(zG, xG);// + PI;
  //else rho = atan2(-zG, xG);// + PI;
  // yaw is arctan(ay/ax)-PI/2 or arctan2(ay, ax)-PI/2
  theta = atan2(yG, xG)-PiOver2;
  //theta = atan2(zG, xG);// + (PI/2);

  deltaTime1 = millis() - previousTime;
  previousTime = millis();

  gx = (mx*x) + bx;
  gy = (my*y) + by;
  gz = (mz*z) + bz;
  
  // returns angle in radians (-PI to +PI), multiply by 360/(2*PI) to convert to degrees
  //if (gz > 0) {
    theta_alt = atan2(sqrt(gx*gx+gy*gy),gz);
    //theta_alt = (float)(fmod(2*PI+atan2(gx,gy), 2*PI));
    //rho_alt = (float)(fmod(2*PI+atan2(gx, sqrt(gy*gy+gz*gz)), 2*PI));
    //rho_alt = atan2(gx,gz);
    rho_alt = fmod(2*PI+atan2(gx,gz), 2*PI);
    phi_alt = fmod(2*PI+atan2(gy,gz), 2*PI);
    //if (rho_alt > PI) rho_alt += (PI/2);
    //if (rho_alt < 0) rho_alt -= (2 * PI);
    //phi_alt = (float)(fmod(2*PI+atan2(gy, sqrt(gx*gx+gz*gz)), 2*PI));
    //phi_alt=0;
    //if (phi_alt > PI) phi_alt += (PI/2);
    //if (phi_alt < 0) phi_alt -= (2 * PI);
  //}
  /*else {
    if ((gx > 0) && (gy > 0)) { //good
      rho_alt = atan2(gx, sqrt(gy*gy+gz*gz));
      phi_alt = PI-atan2(-gy, sqrt(gx*gx+gz*gz));
    }
    if ((gx < 0) && (gy > 0)) { //good+
      rho_alt = atan2(gx, -sqrt(gy*gy+gz*gz));
      phi_alt = atan2(-gy, sqrt(gx*gx+gz*gz));
    }
    if ((gx > 0) && (gy < 0)) { //good
      rho_alt = atan2(gx, sqrt(gy*gy+gz*gz));
      phi_alt = atan2(gy, sqrt(gx*gx+gz*gz));
    }
    if ((gx < 0) && (gy < 0)) { //good
      rho_alt = atan2(gx, sqrt(gy*gy+gz*gz));
      phi_alt = atan2(gy, sqrt(gx*gx+gz*gz));
    }
  }*/
  
  deltaTime2 = millis() - previousTime;
  
  // measuredRate = (RATE-GYROMID+ZERO)*(1000/MAX)
  xRate = ((float)analogRead(XRATEPIN) - GyroMid + xGyroZero) * GyroScaleFactor;
  yRate = ((float)analogRead(YRATEPIN) - GyroMid + yGyroZero) * GyroScaleFactor;
  zRate = ((float)analogRead(YRATEPIN) - GyroMid + zGyroZero) * GyroScaleFactor;
  
  xmitData();
}

float findZero(int channel) {
  float zero = 0;
  for (int i=0; i< 100; i++) zero += analogRead(channel);
  return (zero / 100);
}  

void xmitData() {
  Serial.print(int(xG*1000));
  Serial.print(',');
  Serial.print(int(yG*1000));
  Serial.print(',');
  Serial.print(int(zG*1000));
  Serial.print(',');
  Serial.print(int(rho*1000));
  Serial.print(',');
  Serial.print(int(phi*1000));
  Serial.print(',');
  Serial.print(int(theta*1000));
  Serial.print(',');
  Serial.print(int(gx*1000));
  Serial.print(',');
  Serial.print(int(gy*1000));
  Serial.print(',');
  Serial.print(int(gz*1000));
  Serial.print(',');
  Serial.print(int(rho_alt*1000));
  Serial.print(',');
  Serial.print(int(phi_alt*1000));
  Serial.print(',');
  Serial.print(int(theta_alt*1000));
  Serial.print(',');
  Serial.print(int(deltaTime1));
  Serial.print(',');
  Serial.println(int(deltaTime2));
}

int smooth(int data, float filterVal, float smoothedVal){


  if (filterVal > 1){      // check to make sure param's are within range
    filterVal = .99;
  }
  else if (filterVal <= 0){
    filterVal = 0;
  }

  smoothedVal = (data * (1 - filterVal)) + (smoothedVal  *  filterVal);

  return (int)smoothedVal;
}

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

